#include "armor_recognition.h"
#include "armor_attitude_algorithm.h"
#include "debug.h"
#include "tool.h"


enum
{
	WIDTH_GREATER_THAN_HEIGHT,
	ANGLE_TO_UP
};


//工具函数
RotatedRect& adjustRec(RotatedRect& rec, const int mode);
int svmPredict( Mat & dst_roi, Ptr<SVM> svm);
void draw_light(RotatedRect box, Mat img);
void rotate_arbitrarily_angle(Mat &src,Mat &dst,float angle);
void show_histogram(Mat& img);
void gamma_correction(cv::Mat& src, cv::Mat& dst, float K);
void getColorBinary(Mat src, Mat& dst, bool color_of_enemy, uchar colorThre);

//构造函数
ArmorRecognition::ArmorRecognition(FileStorage fsRead)
{
#ifdef BLUE_ENEMY
	color_of_enemy = 0;
#else
	color_of_enemy = 1;
#endif

	sample_count_ = 0;
#ifdef OPEN_SVM_PREDICT

#ifdef BLUE_ENEMY
	fsRead["svm_param"]["threshold_data"]["s_bgray_th"] >> gray_th_;
	fsRead["svm_param"]["threshold_data"]["s_bcolor_th"] >> color_th_;
#else
	fsRead["svm_param"]["threshold_data"]["s_rgray_th"] >> gray_th_;
	fsRead["svm_param"]["threshold_data"]["s_rcolor_th"] >> color_th_;
#endif


	fsRead["svm_param"]["armor_data"]["s_diff_angle"] >> diff_angle_;

	fsRead["svm_param"]["light_data"]["s_min_area"] >> min_area_;
	fsRead["svm_param"]["light_data"]["s_max_area"] >> max_area_;

    fsRead["svm_param"]["light_data"]["s_min_length"] >> min_length_;
	fsRead["svm_param"]["light_data"]["s_max_length"] >> max_length_;

    fsRead["svm_param"]["light_data"]["s_min_led_ratio"] >> min_led_ratio_;
	fsRead["svm_param"]["light_data"]["s_max_led_ratio"] >> max_led_ratio_;

    fsRead["svm_param"]["armor_data"]["s_min_armor_ratio"] >> min_armor_ratio_;
	fsRead["svm_param"]["armor_data"]["s_max_armor_ratio"] >> max_armor_ratio_;

#else

#ifdef BLUE_ENEMY
	fsRead["normal_param"]["threshold_data"]["bgray_th"] >> gray_th_;
	fsRead["normal_param"]["threshold_data"]["bcolor_th"] >> color_th_;
#else
	fsRead["normal_param"]["threshold_data"]["rgray_th"] >> gray_th_;
	fsRead["normal_param"]["threshold_data"]["rcolor_th"] >> color_th_;
#endif

	fsRead["normal_param"]["armor_data"]["diff_angle"] >> diff_angle_;

	fsRead["normal_param"]["light_data"]["min_area"] >> min_area_;
	fsRead["normal_param"]["light_data"]["max_area"] >> max_area_;

    fsRead["normal_param"]["light_data"]["min_length"] >> min_length_;
	fsRead["normal_param"]["light_data"]["max_length"] >> max_length_;

    fsRead["normal_param"]["light_data"]["min_led_ratio"] >> min_led_ratio_;
	fsRead["normal_param"]["light_data"]["max_led_ratio"] >> max_led_ratio_;

    fsRead["normal_param"]["armor_data"]["min_armor_ratio"] >> min_armor_ratio_;
	fsRead["normal_param"]["armor_data"]["max_armor_ratio"] >> max_armor_ratio_;

#endif


#ifdef DEBUG
	namedWindow("Debug", CV_WINDOW_NORMAL);
	createTrackbar("gray_th", "Debug", &gray_th_, 255, 0);
	createTrackbar("color_th", "Debug", &color_th_, 255, 0);
	createTrackbar("color_of_enemy", "Debug", &color_of_enemy, 1, 0);

	createTrackbar("diff_angle", "Debug", &diff_angle_, 25, 0);
	createTrackbar("min_area", "Debug", &min_area_, 500, 0);
	createTrackbar("max_area", "Debug", &max_area_, 3000, 0);
	createTrackbar("min_length", "Debug", &min_length_, 300, 0);
	createTrackbar("max_length", "Debug", &max_length_, 1000, 0);
	createTrackbar("min_led_ratio", "Debug", &min_led_ratio_, 2, 0);
	createTrackbar("max_led_ratio", "Debug", &max_led_ratio_, 40, 0);
	createTrackbar("min_armor_ratio", "Debug", &min_armor_ratio_, 2, 0);
	createTrackbar("max_armor_ratio", "Debug", &max_armor_ratio_, 10, 0);
#endif


	Mode = 0;
	angle_ = -999.0;
	Sentry = 0;
}










//主处理图像函数
int ArmorRecognition::TrackArmor(Mat frame, Ptr<SVM> svm, Ptr<SVM> sentry, Rect RoiRect, Mat src, int id)
{
	outfile.open("log.txt");
	outfile << "开始处理..." << endl;

	Size imgSize;
	imgSize = frame.size();

	Mat gray = Mat(imgSize, CV_8UC1);	//灰度图
	Mat rimg = Mat(imgSize, CV_8UC1);	//r通道图
	Mat bimg = Mat(imgSize, CV_8UC1);	//b通道图
	Mat gimg = Mat(imgSize, CV_8UC1);
	Mat diff = Mat(imgSize, CV_8UC1);	//通道相减图
	Mat th_g = Mat(imgSize, CV_8UC1);	//灰度二值图
	Mat th_c = Mat(imgSize, CV_8UC1);	//通道相减二值图
	Mat rlt = Mat(imgSize, CV_8UC1);	//最终融合图

	vector<vector<Point>> contour;	//点集合

	RotatedRect light;  //灯条旋转矩形
	vector<RotatedRect> vEllipse;	//灯条集合

	vector<my_rect> vRlt;	//装甲版集合
	my_rect finalarmor;		//最终装甲板

	

//********************************************************************
//*****************************得到rlt图像****************************
//********************************************************************

	cvtColor(frame, gray, COLOR_BGR2GRAY);
	threshold(gray, th_g, gray_th_, 255, THRESH_BINARY);
	dilate(th_g, th_g, Mat(), Point(-1, -1), 1);
	// erode(th_g, th_g, Mat(), Point(-1, -1), 1);

	getColorBinary(frame, th_c, color_of_enemy,  color_th_);
	erode(th_c, th_c, Mat(), Point(-1, -1), 1);
	dilate(th_c, th_c, Mat(), Point(-1, -1), 3);
	
	
#ifdef GRAY_TH_IMG
	imshow("gray_th", th_g);
#endif

#ifdef COLOR_TH_IMG
	imshow("color_th", th_c);
#endif

	bitwise_and(th_c, th_g, rlt);

	outfile << "得到混合图像..." << endl;
#ifdef RLT_IMG
	imshow("rlt", rlt);
#endif
//********************************************************************





//********************************************************************
//****************************得到灯条集合****************************
//********************************************************************


	outfile << "寻找灯条开始..." << endl;

	findContours(rlt, contour, RETR_EXTERNAL, CHAIN_APPROX_NONE);

	for (int k = 0; k < contour.size(); k++)
	{
		double area = contourArea(contour[k]);
		double length = arcLength(contour[k], true); 
		if (area < min_area_ || area > max_area_ || length < min_length_ || length > max_length_)
		{
			continue;
		}
		if (contour[k].size() > 6)
		{
			light = fitEllipse(contour[k]);
			adjustRec(light, ANGLE_TO_UP);
			if (light.angle > 50 || light.angle < -50)
			{
				continue;
			}
			if ( light.size.height / light.size.width > min_led_ratio_ && light.size.height / light.size.width < max_led_ratio_)
			{
				outfile << "得到灯条..." << endl;
				vEllipse.push_back(light);
				
#ifdef DRAW_LIGHT
				draw_rect(light, frame);
#endif
				
			}
		}
		
	}

	contour.clear();
//********************************************************************


//********************************************************************
//***************************得到装甲板集合****************************
//********************************************************************

	outfile << "寻找装甲板开始..." << endl;
	vRlt = ArmorDetect(vEllipse, frame, svm, sentry, id);
	outfile << "得到候选装甲板..." << endl;
//********************************************************************




//********************************************************************
//***************************确定最终装甲板****************************
//********************************************************************

	if (vRlt.size() != 0)
	{
		//判断是否找到了数字出现最多的号码
		bool get_most_num = 0;

#ifdef OPEN_SVM_PREDICT
		//找出历史装甲中次数最多装甲号
		int targetNum=0;
		if(!vnum.empty())
		{
			//装甲号： 1 2 3 4 7 8 9
			int count[10]={0};
			for(auto num:vnum)
				count[num]++;
			// 找出最多出现的次数
			int maxCount=0;
			for(int i = 1; i < 10; i++)  
			{
				if(count[i] > maxCount)
					maxCount = count[i];
			}
			// 找出出现最多次的那个数字
			for(int i = 1; i < 10; i++)            
			{
				if(count[i] == maxCount)
					targetNum = i;
			}
			//保留最近的10个装甲号
			if(vnum.size()>10)
			{
				vnum.erase(vnum.begin());
			}
		}

		for (int i = 0; i < vRlt.size(); i++)
		{
			if (vRlt[i].num == targetNum)
			{
				finalarmor = vRlt[i];
				get_most_num = 1;
				break;
			}
			get_most_num = 0;
		}
#endif

		//找到面积最大的装甲板
		if (!get_most_num)
		{
			finalarmor = vRlt[0];

			if (vRlt.size() > 1)
			{
				for (int i = 1; i < vRlt.size(); i++)
				{
					if (finalarmor.rect.size.area() < vRlt[i].rect.size.area())
					{
						finalarmor = vRlt[i];
					}
				}
			}
		}
		

		outfile << "得到最终装甲板..." << endl;

#ifdef OPEN_SVM_PREDICT
		vnum.push_back(finalarmor.num);
#endif



#ifdef	GET_SAMPLE
		int finalnum;
		finalnum = GetNum(finalarmor.rect, svm, frame);
#endif

		DrawBox(finalarmor.rect, frame, RoiRect);


#ifdef OPEN_SVM_PREDICT
		putText(frame, to_string(finalarmor.num), 
			Point(finalarmor.rect.center.x - 5, finalarmor.rect.center.y + 5), 
			5, 1.2, Scalar(0, 255, 255), 2);
#endif			
//********************************************************************	




		

//********************************************************************
//*****************************数据保护*******************************
//********************************************************************
//int protectData(int &x, int &y, int &width, int &height, int &rows, int &cols);
		try {
			protectData(x_, y_, width_, height_, GHEIGHT, GWIDTH);
		}
		catch (const char* msg) {
			cout << "装甲板roi获取异常";
			x_ = 0;
			y_ = 0;
			width_ = GWIDTH;
			height_ = GHEIGHT;
			cerr << msg << endl;
		}

		
//********************************************************************

		angle_ = finalarmor.rect.angle;

//********************************************************************
//***************************角度距离解算*****************************
//********************************************************************

		ArrorAttitudeAlgorithm Get_data;
		double temp_yaw = 0, temp_pitch = 0;

		outfile << "获取数据..." << endl;

		compensation(vheight ,finalarmor.rect.size.height);
		distance_ = Get_data.angleSover(finalarmor.rect, temp_yaw, temp_pitch, color_of_enemy);
		pitch_ = temp_pitch;
		yaw_ = temp_yaw;

//********************************************************************
		Isfind = 1;
	}
	else 
	{
		outfile << "未找到最终装甲板..." << endl;
		Isfind = 0;
		angle_ = -999.0;
	}

#ifdef FRAME_IMG
	imshow("final", frame);
#endif 	

	vEllipse.clear();
	vRlt.clear();
	outfile << "处理结束..." << endl;
	outfile.close();
	if (Isfind)
	{
#ifdef OPEN_SVM_PREDICT
		return finalarmor.num;
#else
		return 0;
#endif
	}
	else
	{
		return 0;
	}
}













//***************************************************
//*********************成员函数区********************
//***************************************************

//装甲板判定条件
bool ArmorRecognition::Suitble(RotatedRect &r1, RotatedRect &r2)
{
	double dAngle = abs(r1.angle - r2.angle);
	while (dAngle > 180)
	{
		dAngle -= 180;
	}
	if (dAngle < diff_angle_ || 180 - dAngle < diff_angle_)
	{
		float limit;
		if (Sentry == 1)
		{
			limit = 0.5;
		}
		else
		{
			limit = 0.7;
		}
		
		if ((r1.size.height * limit< r2.size.height
			&& r1.size.height * (2 - limit) > r2.size.height)
			||( r2.size.height * limit < r1.size.height
			&& r2.size.height * (2 - limit) > r1.size.height)) // 0.7   1.3
		{
			float armor_width = fabs(r1.center.x - r2.center.x);
			if (armor_width > (r1.size.width + r2.size.width))
			{
				float h_max;
#ifdef OPEN_SVM_PREDICT
				if (Sentry == 1)
				{
					h_max = (r1.size.height + r2.size.height) / 2.0f;
				}
				else
				{
					h_max = (r1.size.height + r2.size.height) / 2.0f * 0.5;
				}
				
#else
				h_max = (r1.size.height + r2.size.height) / 2.0f * 0.3;
#endif
				if (fabs(r1.center.y - r2.center.y) <  h_max)
				{
					return true;
				}
			}
		}
	}
	return false;
}



//获取装甲板数字函数
int ArmorRecognition::GetNum(RotatedRect box, Ptr<SVM> svm, Mat frame)
{
	Point2f pt[4];
	int i;
	for (i = 0; i < 4; i++)
	{
		pt[i].x = 0;
		pt[i].y = 0;
	}
	box.points(pt);
	
	float img_x = box.center.x - box.size.width / 4;
	float img_y = box.center.y - box.size.height;
	float img_wh = box.size.width / 2;
	float img_ht = box.size.height * 2;
	int img_rows = frame.rows;
	int img_cols = frame.cols;

	try {
		protectData(img_x, img_y, img_wh, img_ht, img_rows, img_cols);
	}
	catch (const char* msg) {
		cout << "数字roi获取异常";
		cerr << msg << endl;
		return 0;
	}


	Mat dst_roiROI;
	//cout << "开始构建装甲板数字roi..." << endl;
	dst_roiROI = frame(Rect(img_x, img_y, img_wh, img_ht));
	//cout << "构建装甲板数字roi成功..." << endl;

	Mat dst_roi;
	rotate_arbitrarily_angle(dst_roiROI, dst_roi, box.angle);

	resize(dst_roi, dst_roi, cv::Size(48, 48), (0, 0), (0, 0), cv::INTER_LINEAR);  

	//三通道分别图像均衡化
	Mat dst_roiRGB[3];
	split(dst_roi, dst_roiRGB);
	for (int i = 0; i < 3; i++)
	{
		equalizeHist(dst_roiRGB[i], dst_roiRGB[i]);
	}
	merge(dst_roiRGB, 3, dst_roi);
	//gamma变换 //0.02545
	Mat dst_roiGamma;
	gamma_correction(dst_roi, dst_roiGamma, 3); 


#ifdef NUM_IMG
	imshow("NumRoi", dst_roiGamma);
#endif

#ifdef GET_SAMPLE
	sample_count_++;
	cout << sample_count_<< endl;
	string Img_Name = "sample/"+to_string(sample_count_) + ".jpg";
	imwrite(Img_Name, dst_roiGamma);
#endif
	
	int a = 0;
	a = svmPredict(dst_roiGamma, svm);

	return a;

}

//绘制装甲板，并将装甲板坐标恢复为原图坐标
void  ArmorRecognition::DrawBox(RotatedRect &box, Mat img, Rect RoiRect)
{
	Point2f pt[4];
	int i;
	box.points(pt);
	
#ifdef DRAW_ARMOR	

	line(img, pt[0], pt[1], CV_RGB(0, 255, 255), 3, 8, 0);
	line(img, pt[1], pt[2], CV_RGB(0, 255, 255), 3, 8, 0);
	line(img, pt[2], pt[3], CV_RGB(0, 255, 255), 3, 8, 0);
	line(img, pt[3], pt[0], CV_RGB(0, 255, 255), 3, 8, 0);

#endif

	for (i = 0; i < 4; i++)
	{
		pt[i].x += RoiRect.x;
		pt[i].y += RoiRect.y;
	}
	box.center.x += RoiRect.x;
	box.center.y += RoiRect.y;
	if (box.center.x < 0 || box.center.x > GWIDTH)
	{
		box.center.x = 0;
	}
	if (box.center.y < 0 || box.center.y > GHEIGHT)
	{
		box.center.y = 0;
	} 

	x_ = pt[1].x - (box.size.width * 0.5);
	y_ = pt[1].y - (box.size.height * 0.5);
	width_ = box.size.width * 2;
	height_ = box.size.height * 2;

}



//得到装甲板函数
vector<my_rect> ArmorRecognition::ArmorDetect(vector<RotatedRect> vEllipse, Mat frame, Ptr<SVM> svm, Ptr<SVM> sentry, int id)
{
	vector<my_rect> vRlt;
	my_rect armor;
	int nL, nW;
	double dAngle;
	vRlt.clear();
	if (vEllipse.size() < 2)
	{
		return vRlt;
	}
	for (unsigned int i = 0; i < vEllipse.size() - 1; i++)
	{
		for (unsigned int j = i + 1; j < vEllipse.size(); j++)
		{
			if (Suitble(vEllipse[i], vEllipse[j]))
			{
				armor.rect.center.x = (vEllipse[i].center.x + vEllipse[j].center.x) / 2;
				armor.rect.center.y = (vEllipse[i].center.y + vEllipse[j].center.y) / 2;
				armor.rect.angle = (vEllipse[i].angle + vEllipse[j].angle) / 2;
				nL = (vEllipse[i].size.height + vEllipse[j].size.height) / 2;
				nW = sqrt((vEllipse[i].center.x - vEllipse[j].center.x) * (vEllipse[i].center.x - vEllipse[j].center.x) + (vEllipse[i].center.y - vEllipse[j].center.y) * (vEllipse[i].center.y - vEllipse[j].center.y));

				if (nL < nW)
				{
					armor.rect.size.height = nL;
					armor.rect.size.width = nW;
				}
				else
				{
					armor.rect.size.height = nW;
					armor.rect.size.width = nL;
				}
				adjustRec(armor.rect, WIDTH_GREATER_THAN_HEIGHT);

				//cout << "angle :"<< armor.rect.angle << endl;
				//cout << "Ratio:" << armor.rect.size.width / armor.rect.size.height << endl;
				if (armor.rect.size.width / armor.rect.size.height > min_armor_ratio_
				&& armor.rect.size.width / armor.rect.size.height < max_armor_ratio_
				&& armor.rect.angle > -45
				&& armor.rect.angle < 45 
				)
				{
#ifdef OPEN_SVM_PREDICT
					int finalnum;
					//cout << "开始预测装甲板数字..." << endl;
					if (Sentry == 1)
					{
						finalnum = GetNum(armor.rect, sentry, frame);
					}
					else
					{
						finalnum = GetNum(armor.rect, svm, frame);
					}
					
					//cout << "预测装甲板数字结束..." << endl;
					if (finalnum)
					{
						armor.num = finalnum;
						if (id == 0)
						{
							if ((Sentry == 1) && (finalnum == 7))
							{
								vRlt.push_back(armor);
							}
							else if ((Sentry == 0) 
							&& (((finalnum == 2 || finalnum == 3 || finalnum == 4 )&&(armor.rect.size.width / armor.rect.size.height < 3))
							|| (finalnum == 1 && armor.rect.size.width / armor.rect.size.height > 3)
							|| finalnum == 8 
							|| finalnum == 9))
							{
								vRlt.push_back(armor);
							}
						}
						else if(armor.num == id)
						{
							vRlt.push_back(armor);
						}
					}
#else
					vRlt.push_back(armor);
#endif
				}
			}
		}
	}
	return vRlt;
}









//***************************************************
//*********************工具函数区********************
//***************************************************

//调整矩形角度函数
RotatedRect& adjustRec(RotatedRect& rec, const int mode)
{
	using std::swap;

	float& width = rec.size.width;
	float& height = rec.size.height;
	float& angle = rec.angle;

	if (mode == WIDTH_GREATER_THAN_HEIGHT)
	{
		if (width < height)
		{
			swap(width, height);
			angle += 90.0;
		}
	}

	while (angle >= 90.0) angle -= 180.0;
	while (angle < -90.0) angle += 180.0;

	if (mode == ANGLE_TO_UP)
	{
		if (angle >= 45.0)
		{
			swap(width, height);
			angle -= 90.0;
		}
		else if (angle < -45.0)
		{
			swap(width, height);
			angle += 90.0;
		}
	}
	return rec;
}



//svm预测函数
int svmPredict( Mat & dst_roi, Ptr<SVM> svm)
{

	Mat dstdst_roi = dst_roi.clone();

	Mat src = dst_roi.clone();
	if (!src.data)
	{
		cout << "empty!" << endl;
	}
	Mat trainTempImg = Mat::zeros(Size(48, 48), CV_8UC3);

	resize(src, trainTempImg, trainTempImg.size(), 0, 0);

	HOGDescriptor *hog = new HOGDescriptor(Size(48, 48), Size(8, 8), Size(4, 4), Size(4, 4), 9);
	vector<float>descriptors;     
	hog->compute(trainTempImg, descriptors);  
	//cout << "HOG dims: " << descriptors.size() << endl;  //4356
	Mat SVMtrainMat;
	SVMtrainMat.create(1, descriptors.size(), CV_32FC1);

	int n = 0;
	for (vector<float>::iterator iter = descriptors.begin(); iter != descriptors.end(); iter++)
	{
		SVMtrainMat.at<float>(0, n) = *iter;
		n++;
	}

	int ret = svm->predict(SVMtrainMat);

	return ret;
}



//旋转数字函数
void rotate_arbitrarily_angle(Mat &src,Mat &dst,float angle)
{
    float radian = (float) (angle /180.0 * CV_PI);
 
    //填充图像
    int maxBorder =(int) (max(src.cols, src.rows)* 1.414 ); //即为sqrt(2)*max
    int dx = (maxBorder - src.cols)/2;
    int dy = (maxBorder - src.rows)/2;
    copyMakeBorder(src, dst, dy, dy, dx, dx, BORDER_CONSTANT);
 
    //旋转
    Point2f center( (float)(dst.cols/2) , (float) (dst.rows/2));
    Mat affine_matrix = getRotationMatrix2D( center, angle, 1.0 );//求得旋转矩阵
    warpAffine(dst, dst, affine_matrix, dst.size());
 
    //计算图像旋转之后包含图像的最大的矩形
    float sinVal = abs(sin(radian));
    float cosVal = abs(cos(radian));
    Size targetSize( (int)(src.cols * cosVal +src.rows * sinVal),
                     (int)(src.cols * sinVal + src.rows * cosVal) );
 
    //剪掉多余边框
    int x = (dst.cols - targetSize.width) / 2;
    int y = (dst.rows - targetSize.height) / 2;
    Rect rect(x, y, targetSize.width, targetSize.height);
    dst = Mat(dst,rect);
}


//直方图显示函数
void show_histogram(Mat& img)
{
    //为计算直方图配置变量  
    //首先是需要计算的图像的通道，就是需要计算图像的哪个通道（bgr空间需要确定计算 b或g货r空间）  
    int channels = 0;
    //然后是配置输出的结果存储的 空间 ，用MatND类型来存储结果  
    MatND dstHist;
    //接下来是直方图的每一个维度的 柱条的数目（就是将数值分组，共有多少组）  
    int histSize[] = { 256 };       //如果这里写成int histSize = 256;   那么下面调用计算直方图的函数的时候，该变量需要写 &histSize  
    //最后是确定每个维度的取值范围，就是横坐标的总数  
    //首先得定义一个变量用来存储 单个维度的 数值的取值范围  
    float midRanges[] = { 0, 256 };
    const float *ranges[] = { midRanges };

    calcHist(&img, 1, &channels, Mat(), dstHist, 1, histSize, ranges, true, false);

    //calcHist  函数调用结束后，dstHist变量中将储存了 直方图的信息  用dstHist的模版函数 at<Type>(i)得到第i个柱条的值  
    //at<Type>(i, j)得到第i个并且第j个柱条的值  

    //开始直观的显示直方图——绘制直方图  
    //首先先创建一个黑底的图像，为了可以显示彩色，所以该绘制图像是一个8位的3通道图像  
    Mat drawdst_roi = Mat::zeros(Size(256, 256), CV_8UC3);
    //因为任何一个图像的某个像素的总个数，都有可能会有很多，会超出所定义的图像的尺寸，针对这种情况，先对个数进行范围的限制  
    //先用 minMaxLoc函数来得到计算直方图后的像素的最大个数  
    double g_dHistMaxValue;
    minMaxLoc(dstHist, 0, &g_dHistMaxValue, 0, 0);
    //将像素的个数整合到 图像的最大范围内  
    //遍历直方图得到的数据  
    for (int i = 0; i < 256; i++)
    {
        int value = cvRound(dstHist.at<float>(i) * 256 * 0.9 / g_dHistMaxValue);
        line(drawdst_roi, Point(i, drawdst_roi.rows - 1), Point(i, drawdst_roi.rows - 1 - value), Scalar(255, 255, 255));
    }

    imshow("【b通道均衡化后直方图】", drawdst_roi);
}


//Gamma 矫正
void gamma_correction(cv::Mat& src, cv::Mat& dst, float K) {
	uchar LUT[256];
	src.copyTo(dst);
	for (int i = 0; i < 256; i++) {
		//float f = (i + 0.5f) / 255;
		float f = i / 255.0;
		f = pow(f, K);
		//LUT[i] = cv::saturate_cast<uchar>(f*255.0f-0.5f);
		LUT[i] = cv::saturate_cast<uchar>(f*255.0);
	}

	if (dst.channels() == 1) {
		cv::MatIterator_<uchar> it = dst.begin<uchar>();
		cv::MatIterator_<uchar> it_end = dst.end<uchar>();
		for (; it != it_end; ++it) {
			*it = LUT[(*it)];
		}
	}
	else {
		cv::MatIterator_<cv::Vec3b> it = dst.begin<cv::Vec3b>();
		cv::MatIterator_<cv::Vec3b> it_end = dst.end<cv::Vec3b>();
		for (; it != it_end; ++it) {
			(*it)[0] = LUT[(*it)[0]];
			(*it)[1] = LUT[(*it)[1]];
			(*it)[2] = LUT[(*it)[2]];
		}
	}
}

//颜色通道相减并阈值化
void getColorBinary(Mat src, Mat& dst, bool color_of_enemy, uchar colorThre)
{
	switch (color_of_enemy)
	{
	case 0:
		for (int nI = 0; nI < src.rows; nI++)
		{
			uchar* bgr = src.ptr<uchar>(nI);
			uchar* pix = dst.ptr<uchar>(nI);
			for (int nJ = 0, nK = 0; nJ < src.cols * 3; nJ += 3, nK++)//B:bgr[nJ],G:bgr[nJ+1],R:bgr[nJ+2]
			{
				if ((bgr[nJ] - bgr[nJ+2]) >= colorThre)//B-R
					pix[nK] = 255;
				else
					pix[nK] = 0;
			}
		}
		break;

	case 1:
		for (int nI = 0; nI < src.rows; nI++)
		{
			uchar* bgr = src.ptr<uchar>(nI);
			uchar* pix = dst.ptr<uchar>(nI);
			for (int nJ = 0, nK = 0; nJ < src.cols * 3; nJ += 3, nK++)
			{
				if ((bgr[nJ + 2] - bgr[nJ]) >= colorThre)
					pix[nK] = 255;
				else
					pix[nK] = 0;
			}
		}
		break;
	}
}
